/**
 * @class ObjectMbsRenderer
 *
 * @brief Renders the MBS using the supplied geometries. Basically, with each link
 * 	there is an ModelOBJ object associated. The transformation matrix specifies
 *  the location of the object mesh.
 *
 * @date 26.08.2011
 *
 * @author Tomasz Rudny (rudny@gmail.com)
 * @version 1.0
 */

#include "ObjectMbsRenderer.h"

namespace graph {

using namespace RSIM;

ObjectMbsRenderer::ObjectMbsRenderer() {
	meshes = NULL;
	meshCount = -1;
}

/**
 * Releases allocated memory.
 *
 */
ObjectMbsRenderer::~ObjectMbsRenderer() {
	if (meshes != NULL)
	{
		//-------------------------------------------------------------------------
		// Release the models from memory
		//-------------------------------------------------------------------------
		for (int i = 0; i < meshCount; i++)
			meshes[i].releaseModel();

		delete meshes;
	}
}

/**
 * Allocates the number of meshes for visualization. Basically the idea
 * is that we can assign any number of meshes for the visualization. Then
 * we create a hash map which links the meshes with body id. In this way
 * each body will be rendered using the assigned mesh.
 *
 * @param count the number of meshes to be used (and loaded from files).
 */
void ObjectMbsRenderer::setMeshCount(int count) {
	meshCount = count;
	// allocate memory for meshes
	meshes = new ModelOBJ[meshCount];
}

/**
 * Connects the mesh number with the body ID. Used for rendering.
 * @param meshNumber
 * @param bodyID
 */
void ObjectMbsRenderer::assignMesh(int meshNumber, int bodyID) {
	//TODO: error check
	meshMap[meshNumber] =  bodyID;
}

void ObjectMbsRenderer::setMBS(RSIM::MultibodySystem * _mbs) {
	MbsRenderer::setMBS(_mbs);

	// allocate memory for transformations
	trans = new RSIM::Transform[mbs->getNBodies()];
}

/**
 * Loads the mesh from the given file and assigns it to a link.
 * @param meshFile path and name of the mesh file to be loaded
 * @param position index (number) of the link to be assigned
 */

void ObjectMbsRenderer::loadMesh(const char *meshFile, int position) {
	// you cannot assign to a non-existant body
	if (!mbs || position >= meshCount) {
		fprintf(stderr, "Failed to load a mesh (position: %d) is incorrect.", position);
		return;
	}
	//-------------------------------------------------------------------------
	// Load the mesh from file
	//-------------------------------------------------------------------------
	meshes[position].loadModel(meshFile);
}

/**
 * Renders the MBS using the assigned meshes
 */
void ObjectMbsRenderer::renderMBS() {
	if (wireFrameMode)
			glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	updateTransformations();
}

/**
 * Updates the vector of Transformations that are later used to
 * render all meshes.
 */
void ObjectMbsRenderer::updateTransformations() {
	const RigidBodyNode *Nd = mbs->getRoot();
	const Transform& PM = Nd->data_->getTransformPM();

	recursiveUpdate(Nd, PM.P());
}

/**
 * Draws the whole MBS tree structure recursively
 * @param Nd starting node
 * @param V vector ?
 */
void ObjectMbsRenderer::recursiveUpdate(const RSIM::RigidBodyNode * Nd, const RSIM::Vect3& V)
{
	static Transform prevN;
	// Color of each body is determined by the node id.
	// The color cycles after all colors are used up.
	int nodeid = Nd->getNodeId();
	const RigidBody *Body = Nd->data_;

	const Rotation& R_NF = Body->getRotationNF();
	const InvTransform& iT_FM = InvTransform(Body->getJointTransformMF());

	Vect3 oF = V - R_NF * (iT_FM.P());

	Frame f;
	Transform NF(R_NF, oF);
	f.draw(NF, 0);
	//-------------------------------------------------------------------------
	// Render the appropriate mesh.
	//-------------------------------------------------------------------------
	glColor3f(0.2f, 0.6f, 0.9f);
	Vect3 v = NF.P() - prevN.P();
	meshes[meshMap[nodeid]].drawModel(NF, v.getLength());
	prevN = NF;
	graph::Sphere s(0.3);
	s.draw(NF);
	// Transform T_FB
	const InvTransform& T_FB = InvTransform(Nd->data_->getTransformBF());
	const Mat3x3& R_NB = R_NF * T_FB.R();

	// Frame B
	const Transform& T_NB = Transform(R_NB, oF + R_NF * T_FB.P());
	f.draw(T_NB, 1);

	// Next, to add children M frames
	int len = Nd->Child_.getLength();
	if (len) {
		const Transform *T_FM = Body->getBodyTransformFM();
		for (int i = 0; i < len; ++i) {
			const Vect3& oM = oF + R_NF * T_FM[i].P();
			const Transform& T_NM = Transform(R_NF * T_FM[i].R(), oM);
			f.draw(T_NM, 0);
			recursiveUpdate(Nd->Child_.getNode(i), oM);
		}
	}
}

}
